# coding : UTF-8
# 作者：BingBO   时间：2022年10月17日

import cv2 as cv
import numpy as np
import math
import random
from numpy.linalg import solve  # 解线性方程


# 纵向 灰度重心法
def COG_V(src):
    print("********************** COG_V **********************")
    print("src.shape: ", src.shape)
    height, width, channels = src.shape
    blue, green, red = cv.split(src)
    cv.imshow('red', red)
    cv.imwrite(r"D:\SHU\Research Group\LaserVisionSensor\LaserImage\MVS_Data\20220909\red.jpg", red)

    _, binary_red = cv.threshold(red, 195, 255, cv.THRESH_BINARY)  # ret1分割阈值
    binary_red = cv.medianBlur(binary_red, 3)  # 中值滤波
    cv.imshow('binary', binary_red)
    cv.imwrite(r"D:\SHU\Research Group\LaserVisionSensor\LaserImage\MVS_Data\20220909\binary_red.jpg", binary_red)
    # COG
    cp_row_truth, cp_col, gap_col = [], [], []
    for c in range(width):  # 遍历每一列
        x0, y0 = 0, 0
        for r in range(height):  # 遍历每一行
            x0 += binary_red[r, c]
            y0 += binary_red[r, c] * (r + 1)
        if x0 != 0:
            indexRow = (y0 / x0) - 1  #
            cp_row_truth += [indexRow]
            cp_col += [c]  # 中心点列索引(包含空白区域列坐标的突变)
        else:
            gap_col += [c]  # 缺陷点的列索引(包括了交叉光条右侧 的缺少光条空白区域)
    cp_row = list(map(int, np.around(cp_row_truth).tolist()))
    print("CP_row:", cp_row)
    print("CP_col:", cp_col)
    print("gap_col:", gap_col)

    # 创建显示中心点图像
    cpGlobal = np.zeros_like(src)
    for i in range(0, len(cp_row)):
        src[cp_row[i], cp_col[i]] = [255, 0, 0]  # 在原图上 中心点 蓝色点显示
        cpGlobal[cp_row[i], cp_col[i]] = [255, 255, 255]  #
    cv.imshow("Global cp", cpGlobal)
    cv.imwrite(r"D:\SHU\Research Group\LaserVisionSensor\LaserImage\MVS_Data\20220909\cpImg(COG_V).jpg", cpGlobal)

    gapNums = []
    count = 0
    T = 20
    # 中心点列坐标的突变超过阈值T，认为是工件边缘大断点处，排除偶尔光条的小间断
    for i in range(0, len(cp_col) - 1):  # 中心点的列总数len(CP_col)，索引是0~len(CP_col)-1，下语句后面减去前面，需再减1
        if (cp_col[i + 1] - cp_col[i]) > T:
            # 在原图像上标记左右两断点
            src[cp_row[i], cp_col[i]] = [0, 255, 0]
            src[cp_row[i + 1], cp_col[i + 1]] = [0, 255, 0]
            # (左断点(含中心点)列坐标 , 右断点(含中心点)列坐标 , 左右断点间距)
            gapNums.append((cp_col[i], cp_col[i + 1], (cp_col[i + 1] - cp_col[i] - 1)))
            index = i  # 断裂处的列索引
            count += 1
    # 在原输入图像上显示断点
    cv.imshow('BreakPoint_V', src)
    cv.imwrite(r"D:\SHU\Research Group\LaserVisionSensor\LaserImage\MVS_Data\20220909\BreakPoint_V.png", src)  # 保存图像
    print("共发现间距大于%d像素的(竖直方向)间断数：" % T, count)
    print("gapNums Info:", gapNums)

    # *************取断点左侧 Workpiece点集****************
    cpWorkPiece_row_truth = cp_row_truth[:index + 1]
    cpWorkPiece_row = cp_row[:index + 1]
    cpWorkPiece_col = cp_col[:index + 1]
    cpWorkPiece = [cpWorkPiece_row_truth, cpWorkPiece_col]

    # 创建新图像，显示间断左边中心点
    cpWorkPieceImg = np.zeros_like(src)
    for i in range(0, len(cpWorkPiece_row)):
        cpWorkPieceImg[cpWorkPiece_row[i], cpWorkPiece_col[i]] = [255, 255, 255]
    cv.imshow("cpWorkPieceImg", cpWorkPieceImg)

    return binary_red, cpGlobal, cpWorkPieceImg, cpWorkPiece


# 横向 灰度重心法
def COG_U(binary_red, RowSection):  # binary单通道
    print("  **********************Call: COG_U()  **********************")
    height, width = binary_red.shape
    # RowSection = [min(grooveRight_row), max(grooveRight_row)]
    SectionRow_Min, SectionRow_Max = RowSection[0] + 3, RowSection[1] - 3  # RowSection在RANSAC_Right-COG_U()取到

    # ******************计算获取 cpSection*********************
    cpSection_row, cpSection_col_truth, gapSection_row = [], [], []
    for row in range(SectionRow_Min, SectionRow_Max + 1):  # 遍历区间每一行
        sumx, sumy = 0, 0
        for col in range(width):  # 遍历这一行的每一列
            sumx += binary_red[row, col]
            sumy += binary_red[row, col] * (col + 1)  # 重心在第几列
        if sumx != 0:
            indexCol = (sumy / sumx) - 1  # 化为坐标索引
            cpSection_row += [row]  # r值是从小到大的
            cpSection_col_truth += [indexCol]  # 从小到大（右下）、从大到小（右上）
        else:
            gapSection_row += [row]  # 缺陷点的行索引(包括了交叉光条右侧 的缺少光条空白区域)

    cpSection_col = list(map(int, np.around(cpSection_col_truth).tolist()))
    cpSection = [cpSection_row, cpSection_col_truth]
    print("COG_U共找到区间上的中心点个数:", len(cpSection_row))
    print("cpSection_row:", cpSection_row)
    print("cpSection_col:", cpSection_col)
    if gapSection_row:
        print("右坡口区间 横向COG 检测出间隙")

    # 创建新图像，显示横向中心点
    cpSectionImg = np.zeros_like(src)
    for i in range(0, len(cpSection_row)):
        cpSectionImg[cpSection_row[i], cpSection_col[i]] = [255, 255, 255]
    cv.imshow("cpSection Img", cpSectionImg)
    cv.imwrite(r"D:\SHU\Research Group\LaserVisionSensor\LaserImage\MVS_Data\20220909\cpSection Img.png", cpSectionImg)

    '''
    # *******************计算最小二乘直接拟合直线的 内点数**********************
    polyCoefficpSection = np.polyfit(cpSection_col_truth, cpSection_row, 1)  # np.polyfit(x,y,num)：对一组数据进行多项式拟合
    kc, bc = polyCoefficpSection
    print("(最小二乘拟合)COG_U 右坡口 直线斜率k：", kc)
    print("(最小二乘拟合)COG_U 右坡口 直线截距b：", bc)

    x1, x2 = cpSection_col[0], cpSection_col[-1]
    y1, y2 = round(bc + kc * cpSection_col_truth[0]), round(bc + kc * cpSection_col_truth[-1])
    cv.line(cpSectionImg, (x1, y1), (x2, y2), (0, 255, 0), 1)
    cv.imshow("(LSM)cpSection Img", cpSectionImg)
    cv.imwrite(r"D:\SHU\Research Group\LaserVisionSensor\LaserImage\MVS_Data\20220909\(LSM)cpSectionImg.png",
               cpSectionImg)
    # 计算各个中心点到主线的距离，找出最远特征点的坐标
    count = 0
    for i in range(0, len(cpSection_col)):
        d = math.fabs(kc * cpSection_col_truth[i] - cpSection_row[i] + bc) / math.sqrt(1 + kc * kc)  # 主线方程：kx-y+b=0
        print("d", d)
        if d < 1:
            count += 1
    print("最小二乘法拟合的点数 count:", count)
    # *******************************************
    '''
    # RANSAC获取最佳斜率
    SIZE = len(cpSection_row)
    iterTimes = 1000  # 迭代次数
    best_k, best_b = 0, 0
    pK_num, pV_num = 0, 0  # pK_num:斜线上的点，pV_num:直线上的点

    for i in range(1, iterTimes + 1):
        sampleIndex = random.sample(range(SIZE), 2)  # 每次迭代都 随机截取两个样本，作为数据索引值
        # 随机坐标点(x1，y1)、(x2，y2)
        x1, y1 = cpSection_col_truth[sampleIndex[0]], cpSection_row[sampleIndex[0]]
        x2, y2 = cpSection_col_truth[sampleIndex[1]], cpSection_row[sampleIndex[1]]
        if x2 != x1:
            k = (y2 - y1) / (x2 - x1)
            b = y1 - k * x1
            inCounter1 = 0  # 内点计数器1
            for index in range(SIZE):
                x, y = cpSection_col_truth[index], cpSection_row[index]  # 遍历所有点
                # 数据点到直线的距离 round(yCalcu - y)
                # yCalcu = k * x + b
                d = math.fabs(k * x - y + b) / math.sqrt(1 + k * k)
                # if round(math.fabs(yCalcu - y)) <= 1.5:
                if d <= 1:
                    inCounter1 += 1
            if inCounter1 > pK_num:
                pK_num = inCounter1
                best_k, best_b = k, b
        else:  # x2 == x1
            inCounter2 = 0  # 内点计数器2
            for index in range(SIZE):  # 遍历所有数据点: cpSection_col[0] ~ cpSection_col[SIZE-1]
                x, y = cpSection_col_truth[index], cpSection_row[index]
                if x == x1:  # 即和直线重合的点数
                    inCounter2 += 1
            if inCounter2 > pV_num:  # 记录最大内点数与对应的参数
                pV_num = inCounter2
                C = x2
    print("COG_U RANSAC迭代 %d 次" % i)
    print("pK_num: %d个点, pV_num: %d个点" % (pK_num, pV_num))

    # 如果斜线上的点 或者 直线上的点 >= 最小二乘法拟合的点数
    # if pV_num >= count and pK_num >= count:
    if pV_num >= pK_num:  # pV_num直线上的点 >= pK_num斜线上的点
        print("右坡口为垂直光条 Ar, Br, Cr =", (1, 0, -C))
        Ar, Br, Cr = 1, 0, -C
        # 绘制图像 —— cpSection_row始终是从小到大的
        y1, y2 = SectionRow_Min - 10, SectionRow_Max + 10
        cv.line(cpSectionImg, (round(-Cr), y1), (round(-Cr), y2), (0, 255, 0), 1)
    else:
        print("(RANSAC)右坡口点集非垂直 \nRANSAC主线斜率k: %f, RANSAC主线截距b: %f" % (best_k, best_b))
        Ar, Br, Cr = best_k, -1, best_b
        # 绘制图像cpSection_col大小顺序不确定
        x1, x2 = min(cpSection_col) - 20, max(cpSection_col) + 20
        y1, y2 = round(best_b + best_k * x1), round(best_b + best_k * x2)
        cv.line(cpSectionImg, (x1, y1), (x2, y2), (0, 255, 0), 1)
    # else:  # count>pV_num and count>pK_num
    # Ar, Br, Cr = kc, -1, bc

    polyCoeffiABC_grooveRight = [Ar, Br, Cr]
    cv.imshow("cpSection Img_fitline", cpSectionImg)
    cv.imwrite(r"D:\SHU\Research Group\LaserVisionSensor\LaserImage\MVS_Data\20220909\cpSectionImg_Fitting.png",
               cpSectionImg)
    return cpSection, polyCoeffiABC_grooveRight  # 返回：区间中心点集、右坡口直线拟合系数ABC


# 对工件中心点，最小二乘法拟合主线，提取最远点，分割左右点集
def cpSegmentation(cpWorkPieceImg, cpWorkPiece):
    print("\n********************** cpSegmentation **********************")
    cpWorkPiece_row_truth, cpWorkPiece_col = cpWorkPiece[0], cpWorkPiece[1]
    cpWorkPiece_row = list(map(int, np.around(cpWorkPiece_row_truth).tolist()))
    polyCoeffi = np.polyfit(cpWorkPiece_col, cpWorkPiece_row_truth, 1)  # np.polyfit(x,y,num)：对一组数据进行多项式拟合
    k, b = polyCoeffi
    print("(最小二乘拟合)工件点集 直线斜率k：", k)
    print("(最小二乘拟合)工件点集 直线截距b：", b)

    x1, x2 = cpWorkPiece_col[0], cpWorkPiece_col[-1]
    y1, y2 = round(b + k * x1), round(b + k * x2)
    cv.line(cpWorkPieceImg, (x1, y1), (x2, y2), (0, 255, 0), 1)
    cv.imshow("cpLeft Fitline Img", cpWorkPieceImg)
    cv.imwrite(r"D:\SHU\Research Group\LaserVisionSensor\LaserImage\MVS_Data\20220909\polyfit.png", cpWorkPieceImg)

    # 计算各个中心点到主线的距离，找出最远特征点的坐标
    dMax, index = 0, 0
    for i in range(0, len(cpWorkPiece_row)):
        d = math.fabs(k * cpWorkPiece_col[i] - cpWorkPiece_row_truth[i] + b) / math.sqrt(1 + k * k)  # 主线方程：kx-y+b=0
        if d >= dMax:
            dMax = d
            coor = (cpWorkPiece_row[i], cpWorkPiece_col[i])
            index = i
    print("距离主线最远中心点坐标：", coor)
    cpWorkPieceImg[coor[0], coor[1]] = [0, 0, 255]  # 标记最远点
    cv.imshow("furthest Point", cpWorkPieceImg)
    # cv.imwrite(r"D:\SHU\Research Group\LaserVisionSensor\LaserImage\MVS_Data\20220909\furthestPoint.png", img)  # 保存图像

    # ***********分割最远点左右点集*************
    fpLeft_row, fpLeft_col = cpWorkPiece_row[:index + 1], cpWorkPiece_col[:index + 1]
    fpRight_row, fpRight_col = cpWorkPiece_row[index + 1:], cpWorkPiece_col[index + 1:]
    fpLeft_row_truth, fpRight_row_truth = cpWorkPiece_row_truth[:index + 1], cpWorkPiece_row_truth[index + 1:]

    fpLeft = [fpLeft_row_truth, fpLeft_col]
    fpRight = [fpRight_row_truth, fpRight_col]

    # 创建新图像，显示最远点左边中心点
    fpLeftImg = np.zeros_like(src)
    for i in range(0, len(fpLeft_row)):
        fpLeftImg[fpLeft_row[i], fpLeft_col[i]] = [255, 255, 255]
    cv.imshow("fpLeft Img", fpLeftImg)

    fpRightImg = np.zeros_like(src)
    for i in range(0, len(fpRight_row)):
        fpRightImg[fpRight_row[i], fpRight_col[i]] = [255, 255, 255]
    cv.imshow("fpRight Img", fpRightImg)

    return fpLeftImg, fpRightImg, fpLeft, fpRight


def RANSAC_Left(fpLeftImg, fpLeft):
    print("\n**********************RANSAC_Left**********************")
    fpLeft_row_truth, fpLeft_col = fpLeft
    fpLeft_row = list(map(int, np.around(fpLeft_row_truth).tolist()))
    SIZE = len(fpLeft_row)
    # 选点、评估
    ITERATION = 1000  # 迭代次数
    threshold = 0.8  # 内点数达到总点数的80%
    best_k, best_b = 0, 0  # 最优斜率、最优截距
    pre_total = 0
    # RANSAC找到左半边点的主线斜率
    for i in range(1, ITERATION + 1):
        sampleIndex = random.sample(range(SIZE), 2)  # 每次迭代都 随机截取两个样本，作为数据索引值
        # 随机取得的两个坐标点(x1，y1)、(x2，y2)
        x1, y1 = fpLeft_col[sampleIndex[0]], fpLeft_row_truth[sampleIndex[0]]
        x2, y2 = fpLeft_col[sampleIndex[1]], fpLeft_row_truth[sampleIndex[1]]
        # 随机两点的斜率
        k = (y2 - y1) / (x2 - x1)  # 此处可以改进一下
        b = y1 - k * x1

        total_in = 0  # 内点计数器
        for index in range(SIZE):  # 遍历所有数据点
            x, y = fpLeft_col[index], fpLeft_row_truth[index]
            yCalcu = k * x + b  # 用当前两点的斜率来计算
            if abs(yCalcu - y) <= 1:  # 统计小于偏差epsilon的内点数，符合内点条件
                total_in += 1
            if total_in > pre_total:  # 记录最大内点数与对应的参数
                pre_total = total_in
                best_k, best_b = k, b
            if total_in > SIZE * threshold:  # 内点数大于设定的阈值，提前跳出循环
                break  # 跳出最近一层循环
    print("RANSAC迭代 %d 次  \n(RANSAC)最远点左边点集 RANSAC主线斜率k: %f, RANSAC主线截距b: %f" % (i, best_k, best_b))
    A1, B1, C1 = best_k, -1, best_b
    polyCoeffiABC_fpLeftMainline = [A1, B1, C1]
    print("(RANSAC)最远点左边点 直线ABC系数：", polyCoeffiABC_fpLeftMainline)

    # 绘制左侧主线
    x1, x2 = fpLeft_col[0], fpLeft_col[-1]
    y1, y2 = round(best_b + best_k * x1), round(best_b + best_k * x2)
    cv.line(fpLeftImg, (x1, y1), (x2, y2), (0, 255, 0), 1)
    cv.imshow("fpLeftImg_RANSAC_fitline", fpLeftImg)

    # 计算各个中心点到主线的距离，找出左边坡口上的点
    grooveLeft_row, grooveLeft_col = [], []
    count = 0
    for i in range(0, len(fpLeft_row)):
        d = math.fabs(best_k * fpLeft_col[i] - fpLeft_row_truth[i] + best_b) / math.sqrt(1 + best_k * best_k)
        if d >= 3:
            grooveLeft_row += [fpLeft_row[i]]
            grooveLeft_col += [fpLeft_col[i]]
            count += 1
    print("grooveLeft_row:", grooveLeft_row)
    print("grooveLeft_col:", grooveLeft_col)
    print("左坡口共找到%d个点" % count)

    grooveLeft = [grooveLeft_row, grooveLeft_col]

    # 创建新图像，显示中心点
    grooveLeftImg = np.zeros_like(fpLeftImg)
    for i in range(0, len(grooveLeft_row)):
        grooveLeftImg[grooveLeft_row[i], grooveLeft_col[i]] = [255, 255, 255]
    cv.imshow("grooveLeftImg", grooveLeftImg)
    cv.imwrite(r"D:\SHU\Research Group\LaserVisionSensor\LaserImage\MVS_Data\20220909\grooveLeftImg.png",
               grooveLeftImg)  # 保存图像

    polyCoeffi_grooveLeft = np.polyfit(grooveLeft_col, grooveLeft_row, 1)  # np.polyfit(x,y,num)：对一组数据进行多项式拟合
    k, b = polyCoeffi_grooveLeft
    print("(最小二乘拟合)左坡口 直线斜率k：", k)
    print("(最小二乘拟合)左坡口 直线截距b：", b)
    A2, B2, C2 = k, -1, b
    polyCoeffiABC_grooveLeft = [A2, B2, C2]
    print("(最小二乘拟合)左坡口 直线ABC系数：", polyCoeffiABC_grooveLeft)

    # 显示左坡口拟合线
    x1, x2 = grooveLeft_col[0], grooveLeft_col[-1]
    y1, y2 = round(b + k * x1), round(b + k * x2)
    cv.line(grooveLeftImg, (x1, y1), (x2, y2), (0, 255, 0), 1)
    cv.imshow("(LSM)grooveLeftImg_fitline", grooveLeftImg)
    cv.imwrite(r"D:\SHU\Research Group\LaserVisionSensor\LaserImage\MVS_Data\20220909\grooveLeftImg_fit(1).png",
               grooveLeftImg)

    polyCoeffi_fpLeft = [polyCoeffiABC_fpLeftMainline, polyCoeffiABC_grooveLeft]

    return grooveLeftImg, grooveLeft, polyCoeffi_fpLeft  # 返回：最远点左边点、左坡口直线拟合系数ABC


def RANSAC_Right(fpRightImg, fpRight):
    print("\n**********************RANSAC_Right**********************")
    fpRight_row_truth, fpRight_col = fpRight
    fpRight_row = list(map(int, np.around(fpRight_row_truth).tolist()))
    SIZE = len(fpRight_row)
    # ****************RANSAC求出右半边点集主线******************
    ITERATION = 1000  # 迭代次数
    threshold = 0.8  # 内点数达到总点数的80%
    best_k, best_b = 0, 0  # 最优斜率、最优截距
    pre_total = 0
    for i in range(1, ITERATION + 1):
        sampleIndex = random.sample(range(SIZE), 2)  # 每次迭代都 随机截取两个样本，作为数据索引值
        # 随机取得的两个坐标点(x1，y1)、(x2，y2)
        x1, y1 = fpRight_col[sampleIndex[0]], fpRight_row_truth[sampleIndex[0]]
        x2, y2 = fpRight_col[sampleIndex[1]], fpRight_row_truth[sampleIndex[1]]
        # 随机两点的斜率
        k = (y2 - y1) / (x2 - x1)
        b = y1 - k * x1

        total_in = 0  # 内点计数器
        for index in range(SIZE):  # 遍历所有数据点
            x, y = fpRight_col[index], fpRight_row_truth[index]
            yCalcu = k * x + b  # 用当前两点的斜率来计算
            if abs(yCalcu - y) <= 1:
                total_in += 1
            if total_in > pre_total:  # 记录最大内点数与对应的参数
                pre_total = total_in
                best_k, best_b = k, b
            if total_in > SIZE * threshold:  # 内点数大于设定的阈值，提前跳出循环
                break  # 跳出最近一层循环
    print("RANSAC迭代 %d 次  \n(RANSAC)最远点右边点集 RANSAC主线斜率k: %f, RANSAC主线截距b: %f" % (i, best_k, best_b))
    Ar, Br, Cr = best_k, -1, best_b
    polyCoeffiABC_fpRightMainline = [Ar, Br, Cr]
    print("(RANSAC)最远点右边点 直线ABC系数：", polyCoeffiABC_fpRightMainline)

    # 绘制右侧主线
    x1, x2 = fpRight_col[0], fpRight_col[-1]
    y1, y2 = round(best_b + best_k * x1), round(best_b + best_k * x2)
    cv.line(fpRightImg, (x1, y1), (x2, y2), (0, 255, 0), 1)
    cv.imshow("fpRightImg_RANSAC_fitline", fpRightImg)

    # ********找出右坡口上的点grooveRight***********
    grooveRight_row_truth, grooveRight_col = [], []
    count = 0
    for i in range(0, SIZE):
        d = math.fabs(best_k * fpRight_col[i] - fpRight_row_truth[i] + best_b) / math.sqrt(1 + best_k * best_k)
        if d >= 2.5:
            grooveRight_row_truth += [fpRight_row_truth[i]]
            grooveRight_col += [fpRight_col[i]]
            count += 1
    grooveRight_row = list(map(int, np.around(grooveRight_row_truth).tolist()))
    print("grooveRight_row:", grooveRight_row)
    print("grooveRight_col:", grooveRight_col)
    print("右坡口共找到%d个点" % count)
    # 用最小二乘法拟合得到的有COG_V误差的斜率

    grooveRight = [grooveRight_row_truth, grooveRight_col]
    k, b = np.polyfit(grooveRight_col, grooveRight_row_truth, 1)  # np.polyfit(x,y,num)：对一组数据进行多项式拟合
    print("(最小二乘拟合——含COG_V误差)右坡口 直线斜率k：", k)
    print("(最小二乘拟合——含COG_V误差)右坡口 直线截距b：", b)
    polyCoeffiABC_grooveRight = [k, -1, b]

    # 创建新图像，显示垂直COG得到的 右坡口中心点
    grooveRightImg = np.zeros_like(fpRightImg)
    for i in range(0, len(grooveRight_row)):
        grooveRightImg[grooveRight_row[i], grooveRight_col[i]] = [255, 255, 255]
    cv.imshow("grooveRightImg", grooveRightImg)
    cv.imwrite(r"D:\SHU\Research Group\LaserVisionSensor\LaserImage\MVS_Data\20220909\grooveRight Img.png",
               grooveRightImg)  # 保存图像

    # 用有误差的 右坡口直线斜率k，b 来显示拟合线条
    x1, x2 = grooveRight_col[0], grooveRight_col[-1]
    y1, y2 = round(b + k * x1), round(b + k * x2)
    cv.line(grooveRightImg, (x1, y1), (x2, y2), (0, 255, 0), 1)
    cv.imshow("(LSM)grooveRight Img_fitline", grooveRightImg)
    cv.imwrite(r"D:\SHU\Research Group\LaserVisionSensor\LaserImage\MVS_Data\20220909\grooveRightImg_fitline(1).png",
               grooveRightImg)  # 保存图像

    # **************************当拟合的斜率过大时候，误差较大， 调用COG_U ***************************
    RowSection = [min(grooveRight_row), max(grooveRight_row)]
    if math.fabs(k) > 1.8:
        cpSection, polyCoeffiABC_grooveRight = COG_U(binary, RowSection)  # 更新polyCoeffiABC_grooveRight
        # cpSection = [cpSection_row, cpSection_col_truth]
        grooveRight = cpSection  # 更新grooveRight坐标
    # ***************************************************************

    # 返回：最远点右边点拟合直线系数ABC、右坡口拟合直线系数ABC
    polyCoeffi_fpRight = [polyCoeffiABC_fpRightMainline, polyCoeffiABC_grooveRight]

    return grooveRightImg, grooveRight, polyCoeffi_fpRight


def cpImgDisplay(cpImg, fpLeft, fpRight, grooveLeft, grooveRight, polyCoeffi_fpLeft, polyCoeffi_fpRight):
    print("\n**********************cpImgDisplay**********************")
    # 四部分的 点集坐标
    fpLeft_row_truth, fpLeft_col = fpLeft
    grooveLeft_row, grooveLeft_col = grooveLeft

    grooveRight_row, grooveRight_col_truth = grooveRight  # 这部分点集有误差，只取点集的列坐标上下限
    grooveRight_col = list(map(int, np.around(grooveRight_col_truth).tolist()))

    fpRight_row_truth, fpRight_col = fpRight

    # 四部分的 拟合直线系数
    polyCoeffiABC_fpLeftMainline, polyCoeffiABC_grooveLeft = polyCoeffi_fpLeft
    polyCoeffiABC_fpRightMainline, polyCoeffiABC_grooveRight = polyCoeffi_fpRight

    Al, Bl, Cl = polyCoeffiABC_fpLeftMainline
    Agl, Bgl, Cgl = polyCoeffiABC_grooveLeft
    Agr, Bgr, Cgr = polyCoeffiABC_grooveRight
    Ar, Br, Cr = polyCoeffiABC_fpRightMainline
    print("Agr, Bgr, Cgr:", Agr, Bgr, Cgr)
    # 绘图
    x1, x2 = fpLeft_col[0], grooveLeft_col[0] + 12
    x3, x4 = grooveLeft_col[0] - 40, grooveLeft_col[-1] + 20
    x7, x8 = fpRight_col[0], fpRight_col[len(fpRight_col) - 1]

    y1 = round((-Cl - Al * x1) / Bl)
    y2 = round((-Cl - Al * x2) / Bl)
    cv.line(cpImg, (x1, y1), (x2, y2), (0, 255, 255), 1)  # 黄色

    y3 = round((-Cgl - Agl * x3) / Bgl)
    y4 = round((-Cgl - Agl * x4) / Bgl)
    cv.line(cpImg, (x3, y3), (x4, y4), (34, 139, 34), 1)  # 绿色

    if Bgr == 0:
        y5 = grooveRight_row[0] - 10
        y6 = grooveRight_row[-1] + 10
        cv.line(cpImg, (-Cgr, y5), (-Cgr, y6), (255, 144, 30), 1)  # 粉红
    else:
        x5, x6 = grooveRight_col[0] - 5, grooveRight_col[-1] + 10
        x5_truth, x6_truth = grooveRight_col_truth[0] - 5, grooveRight_col_truth[-1] + 10
        y5 = round((-Cgr - Agr * x5_truth) / Bgr)
        y6 = round((-Cgr - Agr * x6_truth) / Bgr)
        cv.line(cpImg, (x5, y5), (x6, y6), (255, 144, 30), 1)

    y7 = round((-Cr - Ar * x7) / Br)
    y8 = round((-Cr - Ar * x8) / Br)
    cv.line(cpImg, (x7, y7), (x8, y8), (255, 255, 0), 1)  # 青蓝

    # 计算3个特征点坐标
    A1 = np.array([[Al, Bl], [Agl, Bgl]])  # 输出系数矩阵A
    b1 = np.array([-Cl, -Cgl])  # 值
    fp_x1, fp_y1 = solve(A1, b1)
    fp_y1, fp_x1 = round(fp_y1), round(fp_x1)  # a, b = int(x), int(y)  # 直接省略小数部分
    # cpImg[fp_y1, fp_x1] = [255, 0, 255]
    cv.circle(cpImg, [fp_x1, fp_y1], 1, (255, 0, 255), -1)
    print("特征点1坐标：", (fp_y1, fp_x1))

    A2 = np.array([[Agl, Bgl], [Agr, Bgr]])  # 输出系数矩阵A
    b2 = np.array([-Cgl, -Cgr])  # 值
    fp_x2, fp_y2 = solve(A2, b2)
    fp_y2, fp_x2 = round(fp_y2), round(fp_x2)
    # cpImg[fp_y2, fp_x2] = [255, 0, 255]
    cv.circle(cpImg, [fp_x2, fp_y2], 1, (255, 0, 255), -1)
    print("特征点2(焊缝)坐标：", (fp_y2, fp_x2))

    A3 = np.array([[Agr, Bgr], [Ar, Br]])  # 输出系数矩阵A
    b3 = np.array([-Cgr, -Cr])  # 值
    fp_x3, fp_y3 = solve(A3, b3)
    fp_y3, fp_x3 = round(fp_y3), round(fp_x3)
    # cpImg[fp_y3, fp_x3] = [255, 0, 255]
    cv.circle(cpImg, [fp_x3, fp_y3], 1, (255, 0, 255), -1)
    print("特征点3坐标：", (fp_y3, fp_x3))

    cv.imshow("cpImgDisplay", cpImg)
    cv.imwrite(r"D:\SHU\Research Group\LaserVisionSensor\LaserImage\MVS_Data\20220909\cpImgDisplay.png", cpImg)


# src = cv.imread(r"D:\SHU\Research Group\LaserVisionSensor\LaserImage\MVS_Data\20220909\V\Image_20220909111906812.jpg")
src1 = cv.imread(r"D:\SHU\Research Group\LaserVisionSensor\LaserImage\MVS_Data\20220909\laserL1.png")
src2 = cv.imread(r"D:\SHU\Research Group\LaserVisionSensor\LaserImage\MVS_Data\20220909\laserL2.png")
# src = cv.resize(src, None, fx=0.4, fy=0.4, interpolation=cv.INTER_CUBIC)

binary, cpImg, cpWorkPieceImg, cpWorkPiece = COG_V(src1)
fpLeftImg, fpRightImg, fpLeft, fpRight = cpSegmentation(cpWorkPieceImg, cpWorkPiece)

grooveLeftImg, grooveLeft, polyCoeffi_fpLeft = RANSAC_Left(fpLeftImg, fpLeft)
grooveRightImg, grooveRight, polyCoeffi_fpRight = RANSAC_Right(fpRightImg, fpRight)
cpImgDisplay(src1, fpLeft, fpRight, grooveLeft, grooveRight, polyCoeffi_fpLeft, polyCoeffi_fpRight)

cv.waitKey(0)
cv.destroyAllWindows()
